Table of Contents

//SSR_SWITCH represents the Stop/Safe/Ready switch connected to pin 23
//SHIELD_LIMIT represents the limit switch pressed when the safety shield is down connected to pin 5
//R1 represents the RUN1 button connected to pin 25
//R2 represents the RUN1 button connected to pin 25
//E_STOP represents the emergency stop button connected to pin 4
//RWG_LED represents the ready/warning green LED connected to pin 41
//RWR_LED represents the ready/warning green LED connected to pin 39
//RFB_LED represents the run/fault blue LED connected to pin 43
//RFR_LED represents the run/fault red LED connected to pin 45
//MOTOR_A represents motor 2 direction A connected to pin 6
//MOTOR_B represents motor 2 direction B connected to pin 7
//MOTOR_PWM represents motor 2 PWM connected to pin 8
//RELAY represents the motor relay connected to pin 37

//SWITCH represents the Stop/Safe/Ready switch reading (0 when in the ready position and 1 when in the safe/stop position)
//SHIELD represents the shield limit switch reading (0 when pressed and 1 when not pressed)
//RUN1 represents the RUN1 button reading (0 when pressed and 1 when not pressed)
//RUN2 represents the RUN1 button reading (0 when pressed and 1 when not pressed)
//ESTOP represents the emergency stop button reading (0 when pressed and 1 when not pressed)

//SAFE represents the safe state
//FAULT represents the fault state
//READY represents the ready state
//WARNING represents the warning state
//RUNNING represents the running state

//safeToReady represents the transition from the safe to ready state
//readyToWarning represents the transition from the ready to the warning state
//warningLatch represents the latch on the warning state
//warningToRunning represents the transition from the warning to the running state
//warningToReady represents the transition from the warning to the ready state
//safeToFault represents the transition from the safe to the fault state
//readyToFault represents the transition from the ready to the fault state
//warningToFault represents the transition from the warning to the fault state
//runningToFault represents the transition from the running to the fault state
//faultToSafe represents the transition from the fault to the safe state
//readyToSafe represents the transition from the ready to the safe state
//warningToSafe represents the transition from the warning to the safe state
//runningToSafe represents the transition from the running to the safe state
//safeLatch represents the latch on the safe state
//faultLatch represents the latch on the fault state
//readyLatch represents the latch on the ready state
//runningLatch represents the latch on the running state

//TMR1 represents the timer and will return true if the time duration is exceeded
//SWITCH_OLD represents the last state of the stop/safe/ready switch
//ledState represents the state of the LED
//previousMillis represents the last time LED was updated
//interval represents the interval at which to blink the LED (milliseconds)

#include <LiquidCrystal.h> //include LCD library

//initializing input and output pins
const int SSR_SWITCH = 23;
const int SHIELD_LIMIT = 5;
const int R1 = 25;
const int R2 = 27;
const int E_STOP = 4;
const int RWG_LED = 41;
const int RWR_LED = 39;
const int RFB_LED = 43;
const int RFR_LED = 45;
const int MOTOR2_A = 6;
const int MOTOR2_B = 7;
const int MOTOR2_PWM = 8;
const int RELAY = 37;

// Pin numbers for LDC display
int lcd_rs_pin = 14;
int lcd_en_pin = 15;
int lcd_db4_pin = 16;
int lcd_db5_pin = 17;
int lcd_db6_pin = 18;
int lcd_db7_pin = 19;

//initializing states
bool SAFE = false;
bool FAULT = true;
bool READY = false;
bool WARNING = false;
bool RUNNING = false;

// initializing transitions
bool safeToReady = false;
bool readyToWarning = false;
bool warningLatch = false;
bool warningToRunning = false;
bool warningToReady = false;
bool safeToFault = false;
bool readyToFault = false;
bool warningToFault = false;
bool runningToFault = false;
bool faultToSafe = false;
bool readyToSafe = false;
bool warningToSafe = false;
bool runningToSafe = false;
bool safeLatch = false;
bool faultLatch = false;
bool readyLatch = false;
bool runningLatch = false;

// initializing miscellaneous variables
bool TMR1 = false;
bool SWITCH_OLD = true;
int ledState = LOW;
unsigned long previousMillis = 0;
const long interval = 500;

LiquidCrystal LCD(lcd_rs_pin, lcd_en_pin, lcd_db4_pin, lcd_db5_pin, lcd_db6_pin, lcd_db7_pin);

void setup() {
  LCD.begin(8, 2); // initialize LCD
  Serial.begin(9600); //initialize serial communication
  pinMode(SSR_SWITCH, INPUT);
  pinMode(SHIELD_LIMIT, INPUT_PULLUP);
  pinMode(R1, INPUT_PULLUP);
  pinMode(R2, INPUT_PULLUP);
  pinMode(E_STOP, INPUT_PULLUP);
  pinMode(RWG_LED, OUTPUT);
  pinMode(RWR_LED, OUTPUT);
  pinMode(RFB_LED, OUTPUT);
  pinMode(RFR_LED, OUTPUT);
  pinMode(RELAY, OUTPUT);
}

void loop() {
  unsigned long currentMillis = millis(); //defining current time

  // BLOCK 1: Process Inputs
  //----------------------------------------
  int SWITCH = digitalRead(SSR_SWITCH);
  int SHIELD = !digitalRead(SHIELD_LIMIT); //negating so that this variable is 1 when pressed
  int RUN1 = !digitalRead(R1); //negating so that this variable is 1 when pressed
  int RUN2 = !digitalRead(R2); //negating so that this variable is 1 when pressed
  int ESTOP = !digitalRead(E_STOP); //negating so that this variable is 1 when pressed

  //call timer
  TMR1 = Timer(RUN1 && RUN2, 2000);

  // BLOCK 2: State Transition Logic
  //----------------------------------------
  safeToReady = SAFE && !SWITCH;
  readyToWarning = READY && RUN1 && RUN2;
  warningLatch = WARNING && !TMR1 && RUN1 && RUN2 && !SWITCH && !ESTOP && SHIELD;
  warningToRunning = WARNING && TMR1 && RUN1 && RUN2;
  warningToReady = WARNING && !TMR1 && !RUN1 || WARNING && !TMR1 && !RUN2;
  safeToFault = SAFE && ESTOP || SAFE && !SHIELD;
  readyToFault = READY && ESTOP || READY && !SHIELD;
  warningToFault = WARNING && ESTOP || WARNING && !SHIELD;
  runningToFault = RUNNING && ESTOP || RUNNING && !SHIELD;
  faultToSafe = FAULT && !ESTOP && SHIELD && SWITCH && !SWITCH_OLD;
  readyToSafe = READY && SWITCH && !ESTOP && SHIELD;
  warningToSafe = WARNING && SWITCH && !ESTOP && SHIELD;
  runningToSafe = RUNNING && SWITCH && !ESTOP && SHIELD;
  safeLatch = SAFE && SWITCH && !ESTOP && SHIELD;
  faultLatch = FAULT && ESTOP || FAULT && !SHIELD || FAULT && !SWITCH || FAULT && SWITCH_OLD;
  readyLatch = READY && !RUN1 && !ESTOP && SHIELD && !SWITCH || READY && !RUN2 && !ESTOP && SHIELD && !SWITCH;
  runningLatch = RUNNING && !ESTOP && SHIELD && !SWITCH;

  // BLOCK 3: Update States
  //----------------------------------------
  SAFE = faultToSafe || readyToSafe || warningToSafe || runningToSafe || safeLatch;
  FAULT = safeToFault || readyToFault || warningToFault || runningToFault || faultLatch;
  READY = safeToReady || warningToReady || readyLatch;
  WARNING = readyToWarning || warningLatch;
  RUNNING = warningToRunning || runningLatch;

  // BLOCK 4: Set Outputs and Old Variables
  //----------------------------------------

  if (SAFE == 1) {
    //motor disconnected
    digitalWrite(MOTOR2_PWM, LOW);
    digitalWrite(MOTOR2_A, LOW);
    digitalWrite(MOTOR2_B, LOW);

    //READY/WARNING LED illuminated green
    digitalWrite(RFR_LED, LOW);
    digitalWrite(RWR_LED, LOW);
    digitalWrite(RFB_LED, LOW);
    digitalWrite(RWG_LED, HIGH);

    // printing to LCD
    LCD.clear();
    LCD.print("SAFE");
    LCD.setCursor(2, 2);
    LCD.print("DISC");
  }

  if (FAULT == 1) {
    //motor disconnected
    digitalWrite(RELAY, LOW);
    digitalWrite(MOTOR2_PWM, LOW);
    digitalWrite(MOTOR2_A, LOW);
    digitalWrite(MOTOR2_B, LOW);

    //RUN/FAULT LED flashing red and all other LEDs switched off
    if (currentMillis - previousMillis >= interval) {
      previousMillis = currentMillis;
    }
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }
    digitalWrite(RWG_LED, LOW);
    digitalWrite(RWR_LED, LOW);
    digitalWrite(RFB_LED, LOW);
    digitalWrite(RFR_LED, ledState);

    // printing to LCD
    LCD.clear();
    LCD.print("FAULT");
    LCD.setCursor(2, 2);
    LCD.print("DISC");
  }

  if (READY == 1) {
    //motor disconnected
    digitalWrite(RELAY, LOW);
    digitalWrite(MOTOR2_PWM, LOW);
    digitalWrite(MOTOR2_A, LOW);
    digitalWrite(MOTOR2_B, LOW);

    //READY/WARNING LED flashing green and all other LEDs switched off
    if (currentMillis - previousMillis >= interval) {
      previousMillis = currentMillis;
    }
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }
    digitalWrite(RWR_LED, LOW);
    digitalWrite(RFB_LED, LOW);
    digitalWrite(RFR_LED, LOW);
    digitalWrite(RWG_LED, ledState);

    //printing to LCD
    LCD.clear();
    LCD.print("READY");
    LCD.setCursor(2, 2);
    LCD.print("DISC");
  }

  if (WARNING == 1) {
    //motor disconnected
    digitalWrite(RELAY, LOW);
    digitalWrite(MOTOR2_PWM, LOW);
    digitalWrite(MOTOR2_A, LOW);
    digitalWrite(MOTOR2_B, LOW);

    //READY/WARNING LED flashing red and all other LEDs switched off
    if (currentMillis - previousMillis >= interval) {
      previousMillis = currentMillis;
    }
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }
    digitalWrite(RWG_LED, LOW);
    digitalWrite(RFR_LED, LOW);
    digitalWrite(RFB_LED, LOW);
    digitalWrite(RWR_LED, ledState);

    //printing to LCD
    LCD.clear();
    LCD.print("WARNING");
    LCD.setCursor(2, 2);
    LCD.print("DISC");
  }

  if (RUNNING == 1) {
    //motor connected
    digitalWrite(RELAY, HIGH);
    digitalWrite(MOTOR2_PWM, HIGH);
    digitalWrite(MOTOR2_A, LOW);
    digitalWrite(MOTOR2_B, HIGH);

    //RUN/FAULT LED illuminated blue
    digitalWrite(RWG_LED, LOW);
    digitalWrite(RWR_LED, LOW);
    digitalWrite(RFR_LED, LOW);
    digitalWrite(RFB_LED, HIGH);

    //printing to LCD
    LCD.clear();
    LCD.print("RUNNING");
    LCD.setCursor(2, 2);
    LCD.print("CONN");
  }

  //checking if RUN1 or RUN2 is pressed and displaying to LCD
  if (RUN1 == 1) {
    LCD.setCursor(1, 2);
    LCD.print("*");
  } else {
    LCD.setCursor(1, 2);
    LCD.print("-");
  }
  if (RUN2 == 1) {
    LCD.setCursor(6, 2);
    LCD.print("*");
  } else {
    LCD.setCursor(6, 2);
    LCD.print("-");
  }
  // setting "old" switch state
  SWITCH_OLD = SWITCH;
  //delaying 200 ms so that the LCD screen does not flicker
  delay(200);
}

//------------------------------------------------------------------------
// TIMER
//------------------------------------------------------------------------

//TMR1_waiting represents the waiting state
//TMR1_timing represents the timing state

//TMR1_elapsed represents the elapsed time
//TMR1_starttime represents the start of the timing
//TMR1_duration represents the maximum time duration (in this case it is 2 seconds)

//TMRwaitToTime represents the transition from the wait to the timing state
//TMRwaitLatch represents the latch on the waiting state
//TMRtimeToWait represents the transition from the timing to the wait state
//TMRtimeLatch represents the latch on the timing state

boolean TMR1_waiting = true; 
boolean TMR1_timing = false; 
int TMR1_elapsed = 0; 
int TMR1_starttime = millis(); 

bool TMRwaitToTime = false;
bool TMRwaitLatch = false;
bool TMRtimeToWait = false;
bool TMRtimeLatch = false;

bool Timer(bool TMR1_enable, int TMR1_duration) {

  // BLOCK 2: State Transition Logic
  //----------------------------------------
  TMRwaitToTime = TMR1_waiting && TMR1_enable;
  TMRwaitLatch = TMR1_waiting && !TMR1_enable;
  TMRtimeToWait = TMR1_timing && !TMR1_enable || TMR1;
  TMRtimeLatch = TMR1_timing && TMR1_enable && !TMR1;

  // BLOCK 3: Update States
  //----------------------------------------
  TMR1_waiting = TMRwaitLatch || TMRtimeToWait;
  TMR1_timing = TMRwaitToTime || TMRtimeLatch;

  // BLOCK 4: Set Outputs and Old Variables
  //----------------------------------------
  if (TMR1_waiting == 1 ) { // if in waiting state
    TMR1_starttime = millis();
  }
  TMR1_elapsed = millis() - TMR1_starttime; // calculating elapsed time

  if (TMR1_elapsed >= TMR1_duration) { // if the elapsed time is longer than timer duration
    TMR1 = true; 
  }
  if (TMR1_elapsed < TMR1_duration) { // if the elapsed time is less than timer duration
    TMR1 = false;
  }

  return TMR1;
}